/**
 * @file LowLevel.cpp
 * @author Beirnaert Simon
 * @author Thomas Debouche
 * @brief Defines the LowLevel class methods, declared in LowLevel.h.
 */

#include "LowLevel.h"
#include<sstream>
#include <stdlib.h>
#include <string>
using namespace std;

string convertInt(int number); // function to convert an int to a string

LowLevel::LowLevel()  //Constructor code
{
	obj_serial.swrite("q 0 0"); // all motors are off
	obj_serial.swrite("g 0"); // grip is in grab position
}

LowLevel::~LowLevel() //Destructor code 
{
	
}

int LowLevel::move(int leftSpeed, int rightSpeed) // Move code
{
	// convert int to string and assembly the different strings together
	s1 = convertInt(leftSpeed);	
	s2 = convertInt(rightSpeed);
	command = "q " + s1 + " " + s2;

	if (obj_serial.swrite(command) == 1)
	{

		return 1; //failure
	}
	else
	{
		return 0;
	}
}

int LowLevel::grab() //grab code
{
	if(obj_serial.swrite("g 0") == 1)
	{
		return 1; //failure
	}
	else
	{
		return 0;
	}
}

int LowLevel::release() //release code
{
	if(obj_serial.swrite("g 1") == 1)
	{
		return 1; //failure
	}
	else
	{
		return 0;
	}
}

int LowLevel::stop() //stop code
{
	if(obj_serial.swrite("q 0 0") == 1)
	{
		return 1; //failure
	}
	else
	{
		return 0;
	}
}

int LowLevel::getDistance() //getDistance code
{
	if (obj_serial.swrite("u") == 1)
	{
		return -1; // write command failed
	}
	else
	{
	
		if(obj_serial.sread(result) ==-1)
		{
			return -1;	//read command failed
		}
		else
		{
			obj_serial.swrite("u");
			obj_serial.sread(result);
			distance = atoi(result);
			return distance;
		}
	}
}

int LowLevel::getVoltage() //getVoltage code
{	
	
	if (obj_serial.swrite("v") == 1)
	{
		return -1; //write command failed
	}
	else
	{
	
		if(obj_serial.sread(result) ==-1)
		{
			return -1; //read command failed
		}
		else
		{
			//extracts the voltage value from the string and convert it into an int (value in millivolt) 
			sscanf(result,"%*s %d %*s",&voltage);
			voltage = voltage*1000;
			return voltage;
		}
	}
}


string convertInt(int number)
{
   stringstream ss; //create a stringstream
   ss << number;    //add number to the stream
   return ss.str(); //return a string with the contents of the stream
}
